#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S3,     ProtoBoard,     sensorI2CCustomFastSkipStates9V)
#pragma config(Sensor, S4,     IRMux,          sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     RightBack,     tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     LeftBack,      tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     RightFront,    tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     LeftFront,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     LeftLift,      tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C2_2,     RightLift,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C3_1,     TopLift,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S2_C3_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C2_1,    LeftGripper1,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    LeftGripper2,         tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    RightGripper1,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    RightGripper2,        tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    LateralGripper,       tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.

#include "LiftManager.h"						//Automatic lift positioner
#include "3785_motion_functions.h"	//Motion fuctions such as forwards and backwards
#include "IRController.h"						//Include more precise autonomous IR reading function
#include "drivers/lego-touch.h"

void gripperAdjust(int position)
{
	servo[LeftGripper1]=position;
	servo[LeftGripper2]=position;
	servo[RightGripper1]=230-position;
	servo[RightGripper2]=230-position;
}

void initializeRobot()  //Initalizes servos before autonomous starts
{
	gripperAdjust(110);		//Closes the gripper to hold the autonomous ring
	return;
}

int location;  //Variable holding the location of the IR beacon.
const tMUXSensor IRSLeft = msensor_S4_2;	//Variable holding the sensor multiplexer port for the left IR sensor
const tMUXSensor IRSRight = msensor_S4_1;	//Variable holding the sensor multiplexer port for the right IR sensor
const tMUXSensor IRSSide = msensor_S4_4;	//Variable holding the sensor multiplexer port for the side IR sensor
const tMUXSensor touch = msensor_S4_3;		//Variable holding the sensor multiplexer port for the touch sensor

task sensors()
{
	while(true)
	{
		nxtDisplayTextLine(0,"%f",IRGetACDir(IRSLeft));
		nxtDisplayTextLine(1,"%f",IRGetACDir(IRSRight));
		nxtDisplayTextLine(2,"%f",IRGetACDir(IRSSide));
		nxtDisplayTextLine(3,"%i",TSreadState(touch));
		nxtDisplayTextLine(4,"%i",HTPBreadADC(ProtoBoard,0,10));
	}
}

task main()
{

	initializeRobot();  //Initalizes the servos (closes the gripper to hold the autonomous ring
	StartTask(sensors);	//Starts a sensor monitoring task that allows us to check that our sensors work
	waitForStart(); //Wait for the beginning of autonomous phase
	StopTask(sensors);		//Stops the sensor monitoring routine to prevent variable conflicts with our main routine

	goToPosition(410); //Raises the lift to the height of the middle peg: the middle peg is the strageically best position.
	wait10Msec(600);  //Waits four seconds to make sure the lift is settled into position

	if(IRGetACDir(IRSSide)>6.5)	//Checks if the IR beacon is on the closest column
	{
		location = 1;	//Records the column that the IR beacon is on
	}
	else if(IRGetACDir(IRSLeft)>3)  //Checks if the IR beacon is on the farthest column
	{
		location = 3; //Records the column that the IR beacon is on
	}
	else	//If the IR beacon is not on the closest or farthest column, it must be on the middle column
	{
		location = 2; //Records the column that the IR beacon is on
	}
	wait10Msec(5);
	switch(location)
	{
	case 1: //If the IR beacon is on the closest column
		straight(60,8.5); //moves forward ~9 inches to position ourselves directly in front of the column
		break;
	case 2: //If the IR beacon is on the middle column
		straight(60,31); //moves forward ~29 inches to position ourselves directly in front of the column
		break;
	case 3: //If the IR beacon is on the last column
		straight(60,51); //moves forward ~50  inches to position ourselves directly in front of the column
		break;
	}
	stopRobot(); //stops the robot from moving forward
	pause();	//waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	turnRight(50,2200);  //Turns the robot 90 degrees to face the peg rack
	pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	initalizeEncoders();  //clears the encoders so we can accurately read distance
	float dir=0; //Variable holding the relative postion of the IR beacon and the robot
	wait10Msec(20);
	while(distanceNotTraveled(inchesToEncoder(18))
				&& !TSreadState(touch)) //Loop executes will the robot has not travelled 15 inches
																					//(if we get much closer, the robot perpetually turns) and the touch sensor is not pressed
	{
		dir=IRGetACDir(IRSLeft)-(10-IRGetACDir(IRSRight)); //Updates the relative position of the IR beacon so we can turn to it
		if (dir<0) //turns the robot left if the IR beacon is to its left
		{
			leftSpeed(-50);
			rightSpeed(50);
		}
		if (dir>0) //turns the robot right if the IR beacon is to its left
		{
			leftSpeed(50);
			rightSpeed(-50);
		}
		if (dir==0) //moves the robot straight if the IR beacon is directly in front of it
		{
			leftSpeed(30);
			rightSpeed(30);
		}
	}
	dir=IRGetACDir(IRSLeft)-(10-IRGetACDir(IRSRight));  //updates IR one more time to check if we are facing forward
	while (dir<0) //Turns the robot left until it faces forward in case it is not facing straight
	{
		dir=IRGetACDir(IRSLeft)-(10-IRGetACDir(IRSRight));  //Updates the relative position of the IR beacon so we can turn to it
		leftSpeed(-50);
		rightSpeed(50);
	}
	while (dir>0)  //Turns the robot right until it faces forward in case it is not facing straight
	{
		dir=IRGetACDir(IRSLeft)-(10-IRGetACDir(IRSRight));  //Updates the relative position of the IR beacon so we can turn to it
		leftSpeed(50);
		rightSpeed(-50);
	}
	while (!TSreadState(touch)) //Moves the robot forward until the touch sensor hits the column, indicating we are far enough forward
	{
		leftSpeed(30);
		rightSpeed(30);
	}
	stopRobot(); //stops the robot from moving forward
	pause();	//waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	gripperAdjust(150); //opens the grippers to drop the autonomous ring on the peg.
	wait10Msec(200);

	straight(-60,12);  //backs up the robot ~12 inches to get out of the way in case our alliance partner has an autonomous as well
	pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors

	turnLeft(50,2000); //turns the robot to allow us to back up towards the side of the field perimeter
	pause();  //waits half a second to allow the robot to come to a full stop and to reduce the stress on the motors
}
